// Include files.
#include <string>
#include "WPILib.h"
#include "../../SystematicRobot.hpp"
#include "GameService.hpp"

GameService::GameService(SystematicRobot &robot, DriverStation &driverStation) :
	m_robot(robot),
	m_driverStation(driverStation),
	m_modeDisabled(0x0010, "Disabled", false, &DriverStation::InDisabled),
	m_modeAutonomous(0x0011, "Autonomous", true, &DriverStation::InAutonomous),
	m_modeTeleop(0x0012, "Teleop", true, &DriverStation::InOperatorControl),
	m_modeTest(0x0020, "Test", true, &DriverStation::InTest),
	m_modeCurrent(&m_modeDisabled)
{
	AddSubservice(m_serviceEnabled);
	AddMode(m_modeDisabled);
	AddMode(m_modeAutonomous);
	AddMode(m_modeTeleop);
	AddMode(m_modeTest);
}

GameService::~GameService()
{
}

bool GameService::GetIsInEnabledMode()
{
	return m_modeCurrent->GetIsEnabledMode();
}

void GameService::HandlePeriodic()
{
	if (m_robot.IsDisabled() && !GetIsInMode(m_modeDisabled))
		SetMode(m_modeDisabled);
	else if (m_robot.IsEnabled())
	{
		if (m_robot.IsAutonomous() && !GetIsInMode(m_modeAutonomous))
			SetMode(m_modeAutonomous);
		else if (m_robot.IsOperatorControl() && !GetIsInMode(m_modeTeleop))
			SetMode(m_modeTeleop);
		else if (m_robot.IsTest() && !GetIsInMode(m_modeTest))
			SetMode(m_modeTest);
	}
}

bool GameService::GetIsInMode(GameService::Mode &mode)
{
	return m_modeCurrent == &mode;
}

void GameService::SetMode(GameService::Mode &mode)
{
	if (mode.GetIsEnabledMode())
		m_serviceEnabled.Start();
	else
		m_serviceEnabled.Stop();
	
	std::set<Mode*>::iterator it;
	
	for (it = m_modes.begin(); it != m_modes.end(); ++it)
	{
		if ((*it) != &mode)
			(*it)->GetService().Stop();
	}
	
	m_modeCurrent->SetMode(m_driverStation, false);
	mode.SetMode(m_driverStation, true);
	mode.GetService().Start();
	m_modeCurrent = &mode;
	
	printf(std::string("Game: " + mode.GetName() + "\n").c_str());
}

void GameService::AddMode(GameService::Mode &mode)
{
	m_modes.insert(&mode);
	AddSubservice(mode.GetService());
}

GameService::Mode::Mode(UINT32 id, std::string name, bool isEnabledMode, SetModeFunction setMode) :
	m_id(id),
	m_name(name),
	m_isEnabledMode(isEnabledMode),
	m_setMode(setMode)
{
}

UINT32 GameService::Mode::GetId() const
{
	return m_id;
}

std::string GameService::Mode::GetName() const
{
	return m_name;
}

bool GameService::Mode::GetIsEnabledMode() const
{
	return m_isEnabledMode;
}

CompoundService &GameService::Mode::GetService()
{
	return m_service;
}
